﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using componentXtra;
using System.Drawing.Drawing2D;
using Integrador.Controles;

namespace Integrador
{
    public partial class Form1 : Form
    {
        List<double> tiempo = null;
        Dinamica dinamica = null;
        double tiempoTotal;
        double tamanoPaso;
        double[] rot = null;
        int limpiar;
        Estado estadoFinal = null;
        PID pid = null;

        public Form1()
        {
            InitializeComponent();


        }

        private void IniciaModelo()
        {
            Estado estadoInicial = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            estadoFinal = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            rot = new double[4];
            rot.ToList<double>().ForEach(x => x = 0);
            pid = new PID_Z();
            this.dinamica = new Dinamica(rot, estadoInicial);
            //tamaño del paso en segundos(no inferior a 0.01)
            //this.tamanoPaso = timer1.Interval / 1000.0;
            this.tamanoPaso = 0.1;
            this.tiempoTotal = 0;
            this.limpiar = 1;
            pid.Kp = 0;
            pid.Kd = 0;
            pid.Ki = 0;
            pid.Tp = this.tamanoPaso;
       
        }

        private void buttonAcept_Click(object sender, EventArgs e)
        {
            IniciaModelo();
            double alturaDeseada = (double)numericUpDown1.Value;
            pid.errorAnterior = alturaDeseada;
            estadoFinal.Z = alturaDeseada;
            timer1.Enabled = true;


        }


        private void PintaGrafica(List<double> lista, XYGraph grafica)
        {

            grafica.ClearGraphs();
            grafica.AddGraph("", DashStyle.Solid, Color.Yellow, 0, false);
            for (int i = 0; i < lista.Count; i++)
                grafica.AddValue(0, (float)tiempo[i], (float)lista[i]);
            grafica.DrawAll();
        }
        private void PintaGrafica(double tiempo, double valor, XYGraph grafica)
        {

            grafica.AddGraph("", DashStyle.Solid, Color.Yellow, 0, false);
            grafica.AddValue(0, (float)tiempo, (float)valor);
            grafica.DrawAll();
        }
        private void timer1_Tick(object sender, EventArgs e)
        {

            Estado estadoActual = this.dinamica.IntegraPaso(ref this.tiempoTotal, this.tamanoPaso, this.rot);
            PintaGrafica(tiempoTotal, estadoActual.U, this.xyGraph_U);
            PintaGrafica(tiempoTotal, estadoActual.V, this.xyGraph_V);
            PintaGrafica(tiempoTotal, estadoActual.W, this.xyGraph_W);
            PintaGrafica(tiempoTotal, estadoActual.X, this.xyGraph_X);
            PintaGrafica(tiempoTotal, estadoActual.Y, this.xyGraph_Y);
            PintaGrafica(tiempoTotal, estadoActual.Z, this.xyGraph_Z);
            PintaGrafica(tiempoTotal, estadoActual.P, this.xyGraph_P);
            PintaGrafica(tiempoTotal, estadoActual.Q, this.xyGraph_Q);
            PintaGrafica(tiempoTotal, estadoActual.R, this.xyGraph_R);
            PintaGrafica(tiempoTotal, estadoActual.phi, this.xyGraph_Roll);
            PintaGrafica(tiempoTotal, estadoActual.theta, this.xyGraph_Pitch);
            PintaGrafica(tiempoTotal, estadoActual.psi, this.xyGraph_Yaw);
            //para que las graficas no se colapsen
            if (tiempoTotal > limpiar * 100)
            {
                this.LimpiaGraficas();
                limpiar++;
            }
            //pasamos al controlador
            estadoFinal.Z = (double)this.numericUpDown1.Value;
            pid.Kp = (double)this.numericUpDown2.Value;
            pid.Kd = (double)this.numericUpDown3.Value;
            pid.Ki = (double)this.numericUpDown4.Value;
            double[] result = pid.Control(estadoActual, estadoFinal, ref rot);
            PintaGrafica(tiempoTotal, result[0], this.xyGraphProporcional);
            PintaGrafica(tiempoTotal, result[1], this.xyGraphDerivativo);
            PintaGrafica(tiempoTotal, result[2], this.xyGraphIntegrativo);
            PintaGrafica(tiempoTotal, result[3], this.xyGraphError);
            //vemos los rotores
            this.label5.Text = "Rotor1: " + this.rot[0].ToString();
            this.label6.Text = "Rotor2: " + this.rot[1].ToString();
            this.label7.Text = "Rotor3: " + this.rot[2].ToString();
            this.label8.Text = "Rotor4: " + this.rot[3].ToString();

        }
        private void LimpiaGraficas()
        {
            this.xyGraph_U.ClearGraphs();
            this.xyGraph_V.ClearGraphs();
            this.xyGraph_W.ClearGraphs();
            this.xyGraph_X.ClearGraphs();
            this.xyGraph_Y.ClearGraphs();
            this.xyGraph_Z.ClearGraphs();
            this.xyGraph_P.ClearGraphs();
            this.xyGraph_Q.ClearGraphs();
            this.xyGraph_R.ClearGraphs();
            this.xyGraph_Roll.ClearGraphs();
            this.xyGraph_Pitch.ClearGraphs();
            this.xyGraph_Yaw.ClearGraphs();
            this.xyGraphDerivativo.ClearGraphs();
            this.xyGraphError.ClearGraphs();
            this.xyGraphIntegrativo.ClearGraphs();
            this.xyGraphProporcional.ClearGraphs();
        }

        private void button1_Click(object sender, EventArgs e)
        {
            timer1.Enabled = false;
            this.LimpiaGraficas();
        }
    }
}
